package com.math3d;
/**
 * 四元素
 * @Author: clm
 * @Date: 2018-11-12 11:53:04
 */
public class Quaternion {
	public float mW, mX, mY, mZ;
	public static final float s = 1.0f / 2.0f;
	public Quaternion(float w, float x, float y, float z){
		mW = w;
		mX = x;
		mY = y;
		mZ = z;
	}
	public Quaternion(){
		mW = mX = mY = mZ = 0.0f;
	}
	/**
	 * 将指定(x,y,z)欧拉角转为四元素
	 * @param x
	 * @param y
	 * @param z
	 */
	public static final Quaternion eulerAnglesToQuaternion(float x, float y, float z){
		double dx = x * s;
		double dy = y * s;
		double dz = z * s;
		double sinX = Math.sin(dx);
		double sinY = Math.sin(dy);
		double sinZ = Math.sin(dz);
		double cosX = Math.cos(dx);
		double cosY = Math.cos(dy);
		double cosZ = Math.cos(dz);
//		double rx = sinY * sinZ * cosX + cosY * cosZ * sinX;
//		double ry = sinY * cosZ * cosX + cosY * sinZ * sinX;
//		double rz = cosY * sinZ * cosX - sinY * cosZ * sinX;
//		double rw = cosY * cosZ * cosX - sinY * sinZ * sinX;
		double rx = sinX * cosY * cosZ - cosX * sinY * sinZ;
		double ry = cosX * sinY * cosZ + sinX * cosY * sinZ;
		double rz = cosX * cosY * sinZ - sinX * sinY * cosZ;
		double rw = cosX * cosY * cosZ + sinX * sinY * sinZ;
		return new Quaternion((float)rw, (float)rx, (float)ry, (float)rz);
	}
	/**
	 * 将四元素转为矩阵数组
	 * @return
	 */
	public static final float[] converToMat4f(Quaternion q){
		float n = (float) (1.0f / Math.sqrt(q.mX * q.mX + q.mY * q.mY + q.mZ * q.mZ + q.mW * q.mW));
		float qx = q.mX * n;
		float qy = q.mY * n;
		float qz = q.mZ * n;
		float qw = q.mW * n;
		float mat4f[] = new float[16];
		mat4f[0] = 1.0f - 2.0f * qy * qy - 2.0f * qz * qz;
		mat4f[1] = 2.0f * qx * qy - 2.0f * qz * qw;
		mat4f[2] = 2.0f * qx * qz + 2.0f * qy * qw;
		mat4f[3] = 0.0f;
		mat4f[4] = 2.0f * qx * qy + 2.0f * qz * qw;
		mat4f[5] = 1.0f - 2.0f * qx * qx - 2.0f * qz * qz;
		mat4f[6] = 2.0f * qy * qz - 2.0f * qx * qw;
		mat4f[7] = 0.0f;
		mat4f[8] = 2.0f * qx * qz - 2.0f * qy * qw;
		mat4f[9] = 2.0f * qy * qz + 2.0f * qx * qw;
		mat4f[10] = 1.0f - 2.0f * qx * qx - 2.0f * qy * qy;
		mat4f[11] = 0.0f;
		mat4f[12] = 0.0f;
		mat4f[13] = 0.0f;
		mat4f[14] = 0.0f;
		mat4f[15] = 1.0f;
		return mat4f;
	}
	/**
	 * 插值计算
	 * @param q1
	 * @param q2
	 * @param t
	 * @return
	 */
	public static final Quaternion slerp(Quaternion q1, Quaternion q2, float t){
		float dot = q1.mW * q2.mW + q1.mX * q2.mX + q1.mY * q2.mY + q1.mZ * q2.mZ;
		/*如果四元数点积的结果是负值（夹角大于90°），那么后面的插值就会在4D球面上绕远路。为了解决这个问题，先测试点积的结果，当结果是负值时，将2个四元数的其中一个取反（并不会改变它代表的朝向）。而经过这一步操作，可以保证这个旋转走的是最短路径。*/
		Quaternion q0 = null;
		if(dot < 0.0f){
			q0 = new Quaternion();
			q0.mW = -q1.mW;
			q0.mX = -q1.mX;
			q0.mY = -q1.mY;
			q0.mZ = -q1.mZ;
			dot = -dot;
		}
		else
			q0 = q1;
		float k0, k1;

		//当p和q的夹角θ差非常小时会导致sinθ→0，这时除法可能会出现问题。为了避免这样的问题，当θ非常小时可以使用简单的线性插值代替（θ→0时，sinθ≈θ，因此方程退化为线性方程：slerp(p,q,t)=(1-t)p+tq）
		if ( dot > 0.9995f ) {
			k0 = 1.0f - t;
			k1 = t;
		}
		else {
//			float sina = (float) Math.sqrt( 1.0f - dot*dot );
//			float a = (float) Math.atan2( sina, dot );
			float a = (float) Math.acos(dot);
			float sina = (float) Math.sin(a);
			k0 = (float) (Math.sin((1.0f - t)*a)  / sina);
			k1 = (float) (Math.sin(t*a) / sina);
		}
//	    result[0] = q1.mW * k0 + q2.mW *k1;
//	    result[1] = q1.mX * k0 + q2.mX * k1;
//	    result[2] = q1.mY * k0 + q2.mY * k1;
//	    result[3] = q1.mZ * k0 + q2.mZ * k1;
		return new Quaternion(q0.mW * k0 + q2.mW *k1, q0.mX * k0 + q2.mX * k1, q0.mY * k0 + q2.mY * k1, q0.mZ * k0 + q2.mZ * k1);
	}
	@Override
	public String toString() {
		return "Quaternion [mW=" + mW + ", mX=" + mX + ", mY=" + mY + ", mZ="
				+ mZ + "]";
	}
}
